function path = path_plan(q_init,q_goal)
%% PATH_PLAN
% Function that receive in input the initial position, the end position
% (two vectors) and calculate the path to follow. The other parameters are
% the image, and the ratio between pixel and meters of the map.

global workspace_data;

res = 132 * (1/workspace_data.px_to_m_x);
node_path = [];

% Cycle until the node path is empty
while (isempty(node_path) && (res > (1/workspace_data.px_to_m_x)))
    
    res = res/2;
    [ map, n, node_info ] = cells_decomposition([ 1, size(workspace_data.map,1), 1, size(workspace_data.map,1) ], ...
                                                    zeros(size(workspace_data.map)), 0, [], res);

    % Build the graph for the image
%     figure
%     imshow(map)
    g = graph(map,n,node_info);
    
%     size(g)
    
    % A* algortihm for finding the best path
    node_path = a_star_search( (fix(q_init/(1/workspace_data.px_to_m_x)) + 1), ...
                                (fix(q_goal/(1/workspace_data.px_to_m_x)) + 1), ...
                                    g, map, n, node_info );
    
                                node_path;
    path = q_init;
    dir_last = 0;
    workspace_data.chans = [];
    
    
    for i = 2:length(node_path)
        [ channel, dir ] = f_channel( node_path(i - 1), node_path(i), map, node_info );
        workspace_data.chans = [workspace_data.chans; channel];
        
        switch dir
            case 1
                if ( dir_last == 2 )
                    path = [ path; (1/workspace_data.px_to_m_x) * center( node_path(i - 1), node_info ) ];
                end
            case 2
                if ( dir_last == 1 )
                    path = [ path; (1/workspace_data.px_to_m_x) * center( node_path(i - 1), node_info ) ];
                end
            case 3
                if ( dir_last == 4 )
                    path = [ path; (1/workspace_data.px_to_m_x) * center( node_path(i - 1), node_info ) ];
                end
            case 4
                if ( dir_last == 3 )
                    path = [ path; (1/workspace_data.px_to_m_x) * center( node_path(i - 1), node_info ) ];
                end
        end
        path = [ path; (1/workspace_data.px_to_m_x) * channel ];
        dir_last = dir;
    end
    path = [ path; q_goal ];
end

%% Interpolation of the points for generating the correct trajectory 
if ~isempty(node_path)
    
    x = path(:,1);
    y = path(:,2);
    t = zeros(1,size(path,1));
    
    for i = 2:length(x)
        t(i) = t(i - 1) + norm([ x(i) y(i) ] - [ x(i - 1) y(i - 1) ]);
    end
    
    tt = t(1):(1/workspace_data.px_to_m_x):t(end);
    xx = pchip(t, x, tt);
    yy = pchip(t, y, tt);
    
%     path = [ xx', yy'];
% %     
%     v = smooth(v);
%     xx = v(:,1);
%     yy = v(:,2);
%     
%     xx = smooth(xx','moving');
%     yy = smooth(yy','moving');
%     

    xx = smooth(xx','loess');
    yy = smooth(yy','loess');
    
    xx = smooth(xx,'rloess');
    yy = smooth(yy,'rloess');
    
    xx = smooth(xx,'moving');
    yy = smooth(yy,'moving');

    path = [ xx, yy ]
    
%     path = smooth(path(1,:),path(2,:),0.1,'loess');
%     path = smooth(path(1,:),path(2,:),0.1,'rloess');
%     
%     path = [path(1,:), path(2,:)]
    
else
    path = [];
end

end

function [ map, n, node_info ] = cells_decomposition(node, map, n, node_info, res)
%% CELLS_DECOMPOSITION
% Function for decomposing the map. Quadtree decomposition, obstacoles are
% black, freespace it's white. In case a square it's not completely black
% or completely white, the procedure cut the square again.

global workspace_data;

lab = label(node);

switch lab
    
    case 'EMPTY'
        
        n = n + 1;
        
        map(node(1):node(2), node(3):node(4)) = n;
        map(node(1):node(2), node(3):node(4));
        node_info = [ node_info; node ];
        
    case 'MIXED'
        
        if (node(2) - node(1) > round(res/(1/workspace_data.px_to_m_x)))
            
            node_1 = [ node(1), fix((node(1) + node(2) - 1)/2), node(3), fix((node(3) + node(4) - 1)/2) ];
            node_2 = [ node(1), fix((node(1) + node(2) - 1)/2), fix((node(3) + node(4) + 1)/2), node(4) ];
            node_3 = [ fix((node(1) + node(2) + 1)/2), node(2), node(3), fix((node(3) + node(4) - 1)/2) ];
            node_4 = [ fix((node(1) + node(2) + 1)/2), node(2), fix((node(3) + node(4) + 1)/2), node(4) ];
            
            [ map, n, node_info ] = cells_decomposition(node_1, map, n, node_info, res);
            [ map, n, node_info ] = cells_decomposition(node_2, map, n, node_info, res);
            [ map, n, node_info ] = cells_decomposition(node_3, map, n, node_info, res);
            [ map, n, node_info ] = cells_decomposition(node_4, map, n, node_info, res);
            
        end
        
end

end

function lab = label(node)
%% LABEL
% Function for labeling the squares resulted from the Quadtree
% decomposition, and assinging to them 3 differents states; FULL if the
% square is an entire part of an obstacle, EMPTY if the square is an entire
% part of three space, MIXED if is half obstacle half free space. In that
% case we decompose more these squares.

global workspace_data;

if isempty(find(workspace_data.map(node(1):node(2),node(3):node(4)),1))
    lab = 'FULL';
elseif isempty(find(-workspace_data.map(node(1):node(2),node(3):node(4))+1,1))
    lab = 'EMPTY';
else
    lab = 'MIXED';
end
end

function [ channel, dir ] = f_channel(node_1,node_2,map,node_info)

if ( node_info(node_1,1) > 1 )
    border = find(map((node_info(node_1,1) - 1),node_info(node_1,3):node_info(node_1,4)) == node_2) + node_info(node_1,3) - 1;
    if ~isempty(border)
        channel = [ (node_info(node_1,1) - 1) (mean(border) - 1/2) ];
        dir = 1;
        return
    end
end

if ( node_info(node_1,2) < size(map,1) )
    border = find(map((node_info(node_1,2) + 1), ...
                node_info(node_1,3):node_info(node_1,4)) == node_2) ...
                + node_info(node_1,3) - 1;
    if ~isempty(border)
        channel = [ node_info(node_1,2), (mean(border) - 1/2) ];
        dir = 2;
        return
    end
end

if ( node_info(node_1,3) > 1 )
    border = find(map(node_info(node_1,1):node_info(node_1,2), ...
                    (node_info(node_1,3) - 1)) == node_2) ...
                    + node_info(node_1,1) - 1;
    if ~isempty(border)
        channel = [ (mean(border) - 1/2), (node_info(node_1,3) - 1) ];
        dir = 3;
        return
    end
end

if ( node_info(node_1,4) < size(map,2) )
    border = find(map(node_info(node_1,1):node_info(node_1,2), ...
                    (node_info(node_1,4) + 1)) == node_2) ...
                     + node_info(node_1,1) - 1;
    if ~isempty(border)
        channel = [ (mean(border) - 1/2), node_info(node_1,4) ];
        dir = 4;
        return
    end
end

end

function q = center(node,node_info)

q(1) = (node_info(node,1) + node_info(node,2) - 1)/2;
q(2) = (node_info(node,3) + node_info(node,4) - 1)/2;

end